#include <cv.h>
#include <opencv2\highgui\highgui.hpp>
#include <stdio.h>
#include <stdlib.h>
#include<iostream>
#include<fstream>
#include<vector> 
#include <opencv2\imgproc\imgproc.hpp>

using namespace std;
using namespace cv;



int main()
{

	// i

	CvMat *intrinsic = (CvMat*)cvLoad("intrinsic.xml");
	CvMat *distortion = (CvMat*)cvLoad("distortion.xml");
	IplImage *image = 0;
	if ((image = cvLoadImage("3.jpg", CV_LOAD_IMAGE_COLOR)) == 0){
		printf("Error: Couldn't load image\n");
		return -1;
	}
	
	//UNDISTORT OUR IMAGE
	IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
	IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
	cvInitUndistortMap(
		intrinsic,
		distortion,
		mapx,
		mapy
		);
	IplImage *t = cvCloneImage(image);
	cvRemap(t, image, mapx, mapy);

	cvShowImage("Checkers3", image);
	cvSaveImage("checkers3.jpg", image);

	Mat img = cv::cvarrToMat(image);



	if (img.empty()){

		return -1;
	}


	int img_height = img.rows;
	int img_width = img.cols;


	float o, v;

	Mat img_trans = Mat::zeros(img_height, img_width, CV_8UC3);


	               for (int i = 0; i<img_height; i++)
				   {

		                  for (int j = 0; j<img_width; j++)
						  {

		                    	o = (20.6455002*j - 12.9068213*i + 211.207718) / (0.0161536206*j - 0.0697497502*i + 20);
		                    	v = (3.07172060*j + 2.43756366*i + 965.252930) / (0.0161536206*j - 0.0697497502*i + 20);

		                        int x = o;
		                      	int y = v;

			

								if (x<0 || x>(img_width - 1) || y<0 || y>(img_height - 1))
									
									continue;
						
					
			                    uchar*  t = img_trans.ptr<uchar>(i);
			                    uchar* p = img.ptr<uchar>(y);

			                    t[j * 3] = p[x * 3];
			                    t[j * 3 + 1] = p[x *3 + 1];
			                    t[j * 3 + 2] = p[x * 3 + 2];
		                 
		                   }

	                }

    imwrite("niaokan3.jpg", img_trans);
	return 0;
}